/*
 * CHANGE_PI.c
 *
 * Created: 07.11.2012 11:38:35
 *  Author: mariuvi
 */ 

#include "DISPLAY.h"
#include "CHANGE_PD.h"
#include "JOY.h"
#include "definitions.h"

#include <avr/io.h>

static uint8_t state = 0;
static int first = TRUE;
static unsigned int browse = FALSE;

void CHANGE_PD_init()
{
	DISPLAY_clear();
	DISPLAY_write_line_center("Change PD values", 1);
	DISPLAY_write_line("P value: ",4 ,3, 0);
	DISPLAY_int(CHANGE_PD_P_gain, 4, 11);
	
	//Sets up a timer with a 1/64 clock
	BIT_SET(TCCR1B,CS30);
	BIT_SET(TCCR1B,CS31);
	BIT_CLEAR(TCCR1B,CS32);
	
	TCNT1 = 0;
	browse = FALSE;	
	state = 0;
}

int CHANGE_PD_change()
{	
	if(first)
	{
		CHANGE_PD_init();
		first = FALSE;
	}
	
	if((TCNT1 > 62500) && (browse == FALSE))
	{
		browse = TRUE;
	}
	
	//We only browse once every 0.5s
	if(browse == TRUE)
	{					
		JOY_Pos position = JOY_read();
		if(JOY_button_pressed())
		{
			//Check if we're changing P or I
			if(state == 0)
			{
				state = 1;
				DISPLAY_write_line("D value: ",4 ,3, 0);
				DISPLAY_int(CHANGE_PD_D_gain, 4, 11);
			}
			else
			{
				first = TRUE; 	
				return 1;
			}
			TCNT1 = 0;
			browse = FALSE;	
		}
		else if(position.dir == UP)
		{
			if(state == 0)
			{
				CHANGE_PD_P_gain++;
				DISPLAY_int(CHANGE_PD_P_gain, 4, 11);
			}
			else
			{
				CHANGE_PD_D_gain++;
				DISPLAY_int(CHANGE_PD_D_gain, 4, 11);
			}	
			TCNT1 = 0;
			browse = FALSE;
		}
		else if(position.dir == DOWN)
		{
			if(state == 0)
			{
				CHANGE_PD_P_gain--;
				DISPLAY_int(CHANGE_PD_P_gain, 4, 11);
			}
			else
			{
				CHANGE_PD_D_gain--;
				DISPLAY_int(CHANGE_PD_D_gain, 4, 11);
			}	
			TCNT1 = 0;
			browse = FALSE;
		}	
	}
	return 0;				
}